/****************************************************************
 * description      :    graphcut c version                                              
 * author           :    Wang Mingyin
 * time             :     August 14th, 2013
 ****************************************************************/
#include <malloc.h>
#include "graphcut.h"
#include "iostream"

/****************************************************************
 ******************              Block         ******************
 ****************************************************************/
void Block_BlockConstruct(Block *b, int size, void (*err_func)(char *)) {
	b->first = b->last = NULL;
	b->block_size = size;
	b->error_function = err_func;
}
void Block_BlockDeconstruct(struct Block *b) {
	while (b->first) {
		BlockItem *next = b->first->next;
		free(b->first);
		b->first = next;
	}
}

BlockType* Block_New(Block *b, int num) {
	BlockType *t;
	if (!b->last || b->last->current + num > b->last->last) {
		if (b->last && b->last->next) {
			b->last = b->last -> next;
		} else {
			BlockItem *next = (BlockItem *) malloc(sizeof(BlockItem) + (b->block_size-1)*sizeof(BlockType));
			if (!next) { 
				if (b->error_function) {
					(*(b->error_function))("Not enough memory!"); 
				}
				//exit(1); 
			}
			if (b->last) {
				b->last->next = next;
			} else { 
				b->first = next;
			}
			b->last = next;
			b->last->current = &(b->last->data[0]);
			b->last->last = b->last->current + b->block_size;
			b->last->next = NULL;
		}
	}

	t = b->last->current;
	b->last->current += num;
	return t;
}

BlockType *Block_ScanFirst(struct Block *b) {
	for (b->scan_current_block = b->first; b->scan_current_block; b->scan_current_block = b->scan_current_block->next) {
		b->scan_current_data = &(b->scan_current_block->data[0]);
		if (b->scan_current_data < b->scan_current_block->current) {
			return b->scan_current_data++;
		}
	}
	return NULL;
}

	
BlockType *Block_ScanNext(struct Block *b) {
	while (b->scan_current_data >= b->scan_current_block->current) {
		b->scan_current_block = b->scan_current_block->next;
		if (!b->scan_current_block) {
			return NULL;
		}
		b->scan_current_data = &(b->scan_current_block->data[0]);
	}
	return b->scan_current_data++;
}


void Block_Reset(Block *b) {
	BlockItem *bi;
	if (!b->first) {
		return;
	}
	for (bi = b->first; ; bi = bi->next) {
		bi->current = &(bi->data[0]);
		if (bi == b->last) {
			break;
		}
	}
	b->last = b->first;
}
	
void Funcs_Block(Block *b) {
	b->BlockConstruct = Block_BlockConstruct;
	b->BlockDeconstruct = Block_BlockDeconstruct;
	b->New = Block_New;
	b->ScanFirst = Block_ScanFirst;
	b->ScanNext = Block_ScanNext;
}

/****************************************************************
 ******************             DBlock         ******************
 ****************************************************************/

void DBlock_DBlockConstruct(DBlock *db, int size, void (*err_func)(char *)) {
	db->first = NULL;
	db->first_free = NULL;
	db->block_size = size;
	db->error_function = err_func;
}

void DBlock_DBlockDeconstruct(struct DBlock *db) {
	while (db->first) {
		DBlockItem *next = db->first->next;
		free(db->first);
		db->first = next;
	}
}

DBlockType* DBlock_New(struct DBlock *db) {
	//static int g_index = 0;
	DBlockInnerItem *item;
	//printf("%d\n", ++g_index);
	if (!db->first_free) {
		DBlockItem *next = db->first;
		db->first = (DBlockItem *) malloc(sizeof(DBlockItem) + (db->block_size-1)*sizeof(DBlockInnerItem));
		//printf("%d %d\n", ++g_index, sizeof(DBlockItem) + (db->block_size-1)*sizeof(DBlockInnerItem));
		if (!db->first) { 
			if (db->error_function) {
				(*(db->error_function))("Not enough memory!"); 
			}
			//exit(1); 
		}
		db->first_free = &(db->first->data[0]);
		for (item = db->first_free; item < db->first_free + db->block_size-1; item++) {
			item->next_free = item + 1;
		}
		item->next_free = NULL;
		db->first->next = next;
	}

	item = db->first_free;
	db->first_free = item->next_free;
	return (DBlockType *) item;
}

void DBlock_Delete(struct DBlock *db, DBlockType *t) {
	((DBlockInnerItem *)t)->next_free = db->first_free;
	db->first_free = ((DBlockInnerItem *)t);
}

void Func_DBlock(DBlock *db) {
	db->DBlockConstruct = DBlock_DBlockConstruct;
	db->DBlockDeconstruct = DBlock_DBlockDeconstruct;
	db->New = DBlock_New;
	db->Delete = DBlock_Delete;
}



/****************************************************************
 ******************              Graph         ******************
 ****************************************************************/
node_id Graph_add_node(struct Graph *g, int num) {
	node_id result;
	
	if (g->node_last + num > g->node_max) {
		g->reallocate_nodes(g, num);
	}

	if (num == 1) {
		g->node_last->first = NULL;
		g->node_last->tr_cap = 0;
		g->node_last->is_marked = 0;
		g->node_last->is_in_changed_list = 0;

		g->node_last++;
		return g->node_num++;
	} else {
		memset(g->node_last, 0, num * sizeof(Node));

		result = g->node_num;
		g->node_num += num;
		g->node_last += num;
		return result;
	}
}
void Graph_add_edge(struct Graph *g, node_id _i, node_id _j, captype cap, captype rev_cap) {
	struct Arc *a, *a_rev;
	Node *i, *j;

	if (g->arc_last == g->arc_max) {
		g->reallocate_arcs(g);
	}

	a = g->arc_last ++;
	a_rev = g->arc_last ++;

	i = g->nodes + _i;
	j = g->nodes + _j;

	a->sister = a_rev;
	a_rev->sister = a;
	a->next = i->first;
	i->first = a;
	a_rev->next = j->first;
	j->first = a_rev;
	a->head = j;
	a_rev->head = i;
	a->r_cap = cap;
	a_rev->r_cap = rev_cap;
}
void Graph_add_tweights(struct Graph *g, node_id i, tcaptype cap_source, tcaptype cap_sink) {
	tcaptype delta;

	delta = g->nodes[i].tr_cap;
	if (delta > 0) {
		cap_source += delta;
	} else {
		cap_sink   -= delta;
	}
	g->flow += (cap_source < cap_sink) ? cap_source : cap_sink;
	g->nodes[i].tr_cap = cap_source - cap_sink;
}

TermType Graph_what_segment(struct Graph *g, node_id i, TermType default_segm) {
	if (g->nodes[i].parent) {
		return (g->nodes[i].is_sink) ? SINK : SOURCE;
	} else	{
		return default_segm;
	}
}



//advanced
Arc_id Graph_get_first_arc(struct Graph *g) {
	return g->arcs;
}
Arc_id Graph_get_next_arc(struct Graph *g, Arc_id a) {
	return a + 1;
}
int Graph_get_node_num(struct Graph *g) { 
	return g->node_num; 
}
int Graph_get_arc_num(struct Graph *g) { 
	return (int)(g->arc_last - g->arcs); 
}
void Graph_get_arc_ends(struct Graph *g, Arc_id a, node_id *i, node_id *j) {
	*i = (node_id) (a->sister->head - g->nodes);
	*j = (node_id) (a->head - g->nodes);
}
	
tcaptype Graph_get_trcap(struct Graph *g, node_id i) {
	return g->nodes[i].tr_cap;
}

captype Graph_get_rcap(struct Graph *g, struct Arc *a) {
	return a->r_cap;
}

void Graph_set_trcap(struct Graph *g, node_id i, tcaptype trcap) {
	g->nodes[i].tr_cap = trcap;
}
void Graph_set_rcap(struct Graph *g, struct Arc *a, captype rcap) {
	a->r_cap = rcap;
}

void Graph_mark_node(struct Graph *g, node_id _i) {
	Node *i = g->nodes + _i;
	if (!i->next)	{
		/* it's not in the list yet */
		if (g->queue_last[1]) {
			g->queue_last[1]->next = i;
		} else {               
			g->queue_first[1] = i;
		}
		g->queue_last[1] = i;
		i->next = i;
	}
	i->is_marked = 1;
}


void Graph_remove_from_changed_list(struct Graph *g, node_id i) { 
	g->nodes[i].is_in_changed_list = 0;
}


void Graph_set_active(struct Graph *g, Node *i) {
	if (!i->next)	{
		/* it's not in the list yet */
		if (g->queue_last[1]) {
			g->queue_last[1]->next = i;
		} else {               
			g->queue_first[1] = i;
		}
		g->queue_last[1] = i;
		i->next = i;
	}
}
Node* Graph_next_active(struct Graph *g) {
	Node *i;

	while (1) {
		if (!(i = g->queue_first[0])) {
			g->queue_first[0] = i = g->queue_first[1];
			g->queue_last[0]  = g->queue_last[1];
			g->queue_first[1] = NULL;
			g->queue_last[1]  = NULL;
			if (!i) {
				return NULL;
			}
		}

		/* remove it from the active list */
		if (i->next == i) {
			g->queue_first[0] = g->queue_last[0] = NULL;
		} else {              
			g->queue_first[0] = i->next;
		}
		i->next = NULL;

		/* a node in the list is active iff it has a parent */
		if (i->parent) {
			return i;
		}
	}
}


void Graph_set_orphan_front(struct Graph *g, Node* i) {
	Nodeptr *np;
	i->parent = ORPHAN;
	np = g->nodeptr_block->New(g->nodeptr_block);
	np->ptr = i;
	np->next = g->orphan_first;
	g->orphan_first = np;
}
void Graph_set_orphan_rear(struct Graph *g, Node* i) {
	Nodeptr *np;
	i->parent = ORPHAN;
	np = g->nodeptr_block->New(g->nodeptr_block);
	np->ptr = i;
	if (g->orphan_last) {
		g->orphan_last->next = np;
	} else {             
		g->orphan_first = np;
	}
	g->orphan_last = np;
	np->next = NULL;
}

void Graph_add_to_changed_list(struct Graph *g, Node* i) {
	node_id *ptr;
	if (g->changed_list && !i->is_in_changed_list) {
		ptr = g->changed_list->New(g->changed_list, 1); //default num = 1
		*ptr = (node_id)(i - g->nodes);
		i->is_in_changed_list = 1; //true
	}
}

void Func_Graph(struct Graph *g) {
	g->GraphConstruct = Graph_GraphConstruct;
	g->GraphDeconstruct = Graph_GraphDeconstruct;
	g->add_node = Graph_add_node;
	g->add_edge = Graph_add_edge;
	g->add_tweights = Graph_add_tweights;
	g->maxflow = Graph_maxflow;
	g->what_segment = Graph_what_segment;
	g->reallocate_nodes = Graph_reallocate_nodes;
	g->reallocate_arcs = Graph_reallocate_arcs;

	g->reset = Graph_reset;
	g->get_first_arc = Graph_get_first_arc;
	g->get_next_arc = Graph_get_next_arc;

	g->get_node_num = Graph_get_node_num;
	g->get_arc_num = Graph_get_arc_num;
	g->get_arc_ends = Graph_get_arc_ends;

	g->get_trcap = Graph_get_trcap;
	g->get_rcap = Graph_get_rcap;
	g->set_trcap = Graph_set_trcap;
	g->set_rcap = Graph_set_rcap;

	g->mark_node = Graph_mark_node;
	g->remove_from_changed_list = Graph_remove_from_changed_list;

	g->set_active = Graph_set_active;
	g->next_active = Graph_next_active;

	g->set_orphan_front = Graph_set_orphan_front;
	g->set_orphan_rear = Graph_set_orphan_rear;

	g->add_to_changed_list = Graph_add_to_changed_list;
	g->maxflow_init = Graph_maxflow_init;
	g->maxflow_reuse_trees_init = Graph_maxflow_reuse_trees_init;
	g->augment = Graph_augment;
	g->process_source_orphan = Graph_process_source_orphan;
	g->process_sink_orphan = Graph_process_sink_orphan;

	g->test_consistency = Graph_test_consistency;

}

void Graph_GraphConstruct(struct Graph *g, int node_num_max, int edge_num_max, void (*err_func)(char *)) {
	g->node_num = 0;
	g->nodeptr_block = NULL;
	g->error_function = err_func;

	if (node_num_max < 16) {
		node_num_max = 16;
	}
	if (edge_num_max < 16) {
		edge_num_max = 16;
	}

	g->nodes = (Node*) malloc(node_num_max * sizeof(Node));
	g->arcs = (struct Arc*) malloc(2 * edge_num_max * sizeof(struct Arc));
	if (!g->nodes || !g->arcs) { 
		if (g->error_function) {
			(*(g->error_function))("Not enough memory!"); 
		}
		//exit(1); 
	}

	g->node_last = g->nodes;
	g->node_max = g->nodes + node_num_max;
	g->arc_last = g->arcs;
	g->arc_max = g->arcs + 2 * edge_num_max;

	g->maxflow_iteration = 0;
	g->flow = 0;
}
void Graph_GraphDeconstruct(struct Graph *g) {
	if (g->nodeptr_block) { 
		g->nodeptr_block->DBlockDeconstruct(g->nodeptr_block);
		free(g->nodeptr_block); 
		g->nodeptr_block = NULL; 
	}
	free(g->nodes);
	free(g->arcs);
}

void Graph_reallocate_nodes(struct Graph *g, int num) {

	int node_num_max = (int)(g->node_max - g->nodes);
	Node *nodes_old = g->nodes;
	struct Arc* a;

	node_num_max += node_num_max / 2;
	if (node_num_max < g->node_num + num) {
		node_num_max = g->node_num + num;
	}
	g->nodes = (Node*)realloc(nodes_old, node_num_max * sizeof(Node));
	if (!g->nodes) { 
		if (g->error_function) {
			(*(g->error_function))("Not enough memory!"); 
		}
		//exit(1); 
	}

	g->node_last = g->nodes + g->node_num;
	g->node_max = g->nodes + node_num_max;

	if (g->nodes != nodes_old) {
		for (a = g->arcs; a < g->arc_last; a++)	{
			a->head = (Node*) ((char*)a->head + (((char*) g->nodes) - ((char*) nodes_old)));
		}
	}
}

void Graph_reallocate_arcs(struct Graph *g) {
	int arc_num_max = (int)(g->arc_max - g->arcs);
	int arc_num = (int)(g->arc_last - g->arcs);
	struct Arc* arcs_old = g->arcs;
	Node* i;
	struct Arc* a;

	arc_num_max += arc_num_max / 2; 
	if (arc_num_max & 1) {
		arc_num_max ++;
	}
	g->arcs = (struct Arc*) realloc(arcs_old, arc_num_max * sizeof(struct Arc));
	if (!g->arcs) { 
		if (g->error_function) {
			(*(g->error_function))("Not enough memory!"); 
		}
		//exit(1); 
	}

	g->arc_last = g->arcs + arc_num;
	g->arc_max = g->arcs + arc_num_max;

	if (g->arcs != arcs_old) {
		for (i = g->nodes; i < g->node_last; i++) {
			if (i->first) {
				i->first = (struct Arc*) ((char*)i->first + (((char*) g->arcs) - ((char*) arcs_old)));
			}
		}
		for (a = g->arcs; a < g->arc_last; a++) {
			if (a->next) {
				a->next = (struct Arc*) ((char*)a->next + (((char*) g->arcs) - ((char*) arcs_old)));
			}
			a->sister = (struct Arc*) ((char*)a->sister + (((char*) g->arcs) - ((char*) arcs_old)));
		}
	}
}
void Graph_reset(struct Graph *g) {
	g->node_last = g->nodes;
	g->arc_last = g->arcs;
	g->node_num = 0;

	if (g->nodeptr_block) { 
		g->nodeptr_block->DBlockDeconstruct(g->nodeptr_block);
		free(g->nodeptr_block); 
		g->nodeptr_block = NULL; 
	}

	g->maxflow_iteration = 0;
	g->flow = 0;
}



/********************************************************************************
 *************                       maxflow                    *****************
 ********************************************************************************/
void Graph_maxflow_init(struct Graph *g) {
	Node *i;

	g->queue_first[0] = g->queue_last[0] = NULL;
	g->queue_first[1] = g->queue_last[1] = NULL;
	g->orphan_first = NULL;

	g->TIME = 0;

	for (i = g->nodes; i < g->node_last; i++) {
		i->next = NULL;
		i->is_marked = 0;
		i->is_in_changed_list = 0;
		i->TS = g->TIME;
		if (i->tr_cap > 0) {
			/* i is connected to the source */
			i->is_sink = 0;
			i->parent = TERMINAL;
			g->set_active(g, i);
			i->DIST = 1;
		} else if (i->tr_cap < 0) {
			/* i is connected to the sink */
			i->is_sink = 1;
			i->parent = TERMINAL;
			g->set_active(g, i);
			i->DIST = 1;
		} else	{
			i->parent = NULL;
		}
	}
}
void Graph_maxflow_reuse_trees_init(struct Graph *g) {
	Node* i;
	Node* j;
	Node* queue = g->queue_first[1];
	struct Arc* a;
	Nodeptr* np;

	g->queue_first[0] = g->queue_last[0] = NULL;
	g->queue_first[1] = g->queue_last[1] = NULL;
	g->orphan_first = g->orphan_last = NULL;

	g->TIME++;

	while ((i = queue)) {
		queue = i->next;
		if (queue == i) {
			queue = NULL;
		}
		i->next = NULL;
		i->is_marked = 0;
		g->set_active(g, i);

		if (i->tr_cap == 0) {
			if (i->parent) {
				g->set_orphan_rear(g, i);
			}
			continue;
		}

		if (i->tr_cap > 0) {
			if (!i->parent || i->is_sink) {
				i->is_sink = 0;
				for (a = i->first; a; a = a->next) {
					j = a->head;
					if (!j->is_marked) {
						if (j->parent == a->sister) {
							g->set_orphan_rear(g, j);
						}
						if (j->parent && j->is_sink && a->r_cap > 0) {
							g->set_active(g, j);
						}
					}
				}
				g->add_to_changed_list(g, i);
			}
		} else	{
			if (!i->parent || !i->is_sink) {
				i->is_sink = 1;
				for (a = i->first; a; a = a->next) {
					j = a->head;
					if (!j->is_marked) {
						if (j->parent == a->sister) {
							g->set_orphan_rear(g, j);
						}
						if (j->parent && !j->is_sink && a->sister->r_cap > 0) {
							g->set_active(g, j);
						}
					}
				}
				g->add_to_changed_list(g, i);
			}
		}
		i->parent = TERMINAL;
		i->TS = g->TIME;
		i->DIST = 1;
	}

	//test_consistency();

	/* adoption */
	while ((np = g->orphan_first)) {
		g->orphan_first = np->next;
		i = np->ptr;
		g->nodeptr_block->Delete(g->nodeptr_block, np);
		if (!g->orphan_first) {
			g->orphan_last = NULL;
		}
		if (i->is_sink) {
			g->process_sink_orphan(g, i);
		} else {            
			g->process_source_orphan(g, i);
		}
	}
	/* adoption end */

	//test_consistency();
}

void Graph_augment(struct Graph *g, struct Arc *middle_arc) {
	Node *i;
	struct Arc *a;
	tcaptype bottleneck;


	/* 1. Finding bottleneck capacity */
	/* 1a - the source tree */
	bottleneck = middle_arc->r_cap;
	for (i = middle_arc->sister->head; ; i = a->head) {
		a = i->parent;
		if (a == TERMINAL) {
			break;
		}
		if (bottleneck > a->sister->r_cap) {
			bottleneck = a->sister->r_cap;
		}
	}
	if (bottleneck > i->tr_cap) {
		bottleneck = i->tr_cap;
	}
	/* 1b - the sink tree */
	for (i = middle_arc->head; ; i = a->head) {
		a = i->parent;
		if (a == TERMINAL) {
			break;
		}
		if (bottleneck > a->r_cap) {
			bottleneck = a->r_cap;
		}
	}
	if (bottleneck > - i->tr_cap) {
		bottleneck = - i->tr_cap;
	}

	/* 2. Augmenting */
	/* 2a - the source tree */
	middle_arc->sister->r_cap += bottleneck;
	middle_arc->r_cap -= bottleneck;
	for (i = middle_arc->sister->head; ; i = a->head) {
		a = i->parent;
		if (a == TERMINAL) {
			break;
		}
		a->r_cap += bottleneck;
		a->sister->r_cap -= bottleneck;
		if (!a->sister->r_cap) {
			g->set_orphan_front(g, i); // add i to the beginning of the adoption list
		}
	}
	i->tr_cap -= bottleneck;
	if (!i->tr_cap)	{
		g->set_orphan_front(g, i); // add i to the beginning of the adoption list
	}
	/* 2b - the sink tree */
	for (i = middle_arc->head; ; i = a->head) {
		a = i->parent;
		if (a == TERMINAL) {
			break;
		}
		a->sister->r_cap += bottleneck;
		a->r_cap -= bottleneck;
		if (!a->r_cap) {
			g->set_orphan_front(g, i); // add i to the beginning of the adoption list
		}
	}
	i->tr_cap += bottleneck;
	if (!i->tr_cap)	{
		g->set_orphan_front(g, i); // add i to the beginning of the adoption list
	}
	g->flow += bottleneck;
}
void Graph_process_source_orphan(struct Graph *g, Node *i) {
	Node *j;
	struct Arc *a0, *a0_min = NULL, *a;
	int d, d_min = INFINITE_D;

	/* trying to find a new parent */
	for (a0 = i->first; a0; a0 = a0->next) {
		if (a0->sister->r_cap) {
			j = a0 -> head;
			if (!j->is_sink && (a = j->parent)) {
				/* checking the origin of j */
				d = 0;
				while (1) {
					if (j->TS == g->TIME) {
						d += j->DIST;
						break;
					}
					a = j->parent;
					d++;
					if (a==TERMINAL) {
						j->TS = g->TIME;
						j->DIST = 1;
						break;
					}
					if (a == ORPHAN) { 
						d = INFINITE_D; 
						break; 
					}
					j = a->head;
				}
				if (d < INFINITE_D) { /* j originates from the source - done */
					if (d<d_min) {
						a0_min = a0;
						d_min = d;
					}
					/* set marks along the path */
					for (j = a0->head; j->TS != g->TIME; j = j->parent->head)	{
						j->TS = g->TIME;
						j->DIST = d --;
					}
				}
			}
		}
	}

	if (i->parent = a0_min) {
		i->TS = g->TIME;
		i->DIST = d_min + 1;
	} else {
		/* no parent is found */
		g->add_to_changed_list(g, i);

		/* process neighbors */
		for (a0 = i->first; a0; a0 = a0->next) {
			j = a0->head;
			if (!j->is_sink && (a = j->parent)) {
				if (a0->sister->r_cap) {
					g->set_active(g, j);
				}
				if (a != TERMINAL && a != ORPHAN && a->head == i) {
					g->set_orphan_rear(g, j); // add j to the end of the adoption list
				}
			}
		}
	}
}
void Graph_process_sink_orphan(struct Graph *g, Node *i) {
	Node *j;
	struct Arc *a0, *a0_min = NULL, *a;
	int d, d_min = INFINITE_D;

	/* trying to find a new parent */
	for (a0 = i->first; a0; a0 = a0->next) {
		if (a0->r_cap) {
			j = a0 -> head;
			if (j->is_sink && (a = j->parent)) {
				/* checking the origin of j */
				d = 0;
				while (1) {
					if (j->TS == g->TIME) {
						d += j->DIST;
						break;
					}
					a = j->parent;
					d++;
					if (a==TERMINAL)	{
						j->TS = g->TIME;
						j->DIST = 1;
						break;
					}
					if (a==ORPHAN) { 
						d = INFINITE_D; 
						break; 
					}
					j = a->head;
				}
				if (d<INFINITE_D) { /* j originates from the sink - done */
					if (d<d_min) {
						a0_min = a0;
						d_min = d;
					}

					/* set marks along the path */
					for (j = a0->head; j->TS != g->TIME; j = j->parent->head) {
						j->TS = g->TIME;
						j->DIST = d --;
					}
				}
			}
		}
	}

	if (i->parent = a0_min) {
		i->TS = g->TIME;
		i->DIST = d_min + 1;
	} else {
		/* no parent is found */
		g->add_to_changed_list(g, i);

		/* process neighbors */
		for (a0 = i->first; a0; a0 = a0->next) {
			j = a0->head;
			if (j->is_sink && (a = j->parent)) {
				if (a0->r_cap) {
					g->set_active(g, j);
				}
				if (a != TERMINAL && a != ORPHAN && a->head == i) {
					g->set_orphan_rear(g, j); // add j to the end of the adoption list
				}
			}
		}
	}
}

flowtype Graph_maxflow(struct Graph *g, int reuse_trees, Block *_changed_list) {
	Node *i, *j, *current_node = NULL;
	struct Arc *a;
	Nodeptr *np, *np_next;

	if (!g->nodeptr_block) {
		g->nodeptr_block = (DBlock *) malloc(sizeof(DBlock));
		Func_DBlock(g->nodeptr_block);
		g->nodeptr_block->DBlockConstruct(g->nodeptr_block, NODEPTR_BLOCK_SIZE, g->error_function);
	}

	g->changed_list = _changed_list;
	if (g->maxflow_iteration == 0 && reuse_trees) { 
		if (g->error_function) {
			(*(g->error_function))("reuse_trees cannot be used in the first call to maxflow()!"); 
		}
		//exit(1); 
	}
	if (g->changed_list && !reuse_trees) { 
		if (g->error_function) {
			(*(g->error_function))("changed_list cannot be used without reuse_trees!"); 
		}
		//exit(1); 
	}

	if (reuse_trees) {
		g->maxflow_reuse_trees_init(g);
	} else {             
		g->maxflow_init(g);
	}

	// main loop
	while (1) {
		// test_consistency(current_node);

		if ((i = current_node)) {
			i->next = NULL; /* remove active flag */
			if (!i->parent) {
				i = NULL;
			}
		}
		if (!i) {
			if (!(i = g->next_active(g))) {
				break;
			}
		}

		/* growth */
		if (!i->is_sink) {
			/* grow source tree */
			for (a = i->first; a; a = a->next)
				if (a->r_cap) {
					j = a->head;
					if (!j->parent) {
						j->is_sink = 0;
						j->parent = a->sister;
						j->TS = i->TS;
						j->DIST = i->DIST + 1;
						g->set_active(g, j);
						g->add_to_changed_list(g, j);
					} else if (j->is_sink) {
						break;
					} else if (j->TS <= i->TS && j->DIST > i->DIST) {
						/* heuristic - trying to make the distance from j to the source shorter */
						j->parent = a->sister;
						j->TS = i->TS;
						j->DIST = i->DIST + 1;
					}
				}
		} else {
			/* grow sink tree */
			for (a = i->first; a; a = a->next)
				if (a->sister->r_cap) {
					j = a->head;
					if (!j->parent) {
						j->is_sink = 1;
						j->parent = a->sister;
						j->TS = i->TS;
						j->DIST = i->DIST + 1;
						g->set_active(g, j);
						g->add_to_changed_list(g, j);
					} else if (!j->is_sink) { 
						a = a->sister; 
						break; 
					} else if (j->TS <= i->TS &&	j->DIST > i->DIST)	{
						/* heuristic - trying to make the distance from j to the sink shorter */
						j->parent = a->sister;
						j->TS = i->TS;
						j->DIST = i->DIST + 1;
					}
				}
		}

		g->TIME ++;

		if (a) {
			i->next = i; /* set active flag */
			current_node = i;

			/* augmentation */
			g->augment(g, a);
			/* augmentation end */

			/* adoption */
			while ((np = g->orphan_first)) {
				np_next = np->next;
				np->next = NULL;

				while ((np = g->orphan_first)) {
					g->orphan_first = np -> next;
					i = np -> ptr;
					g->nodeptr_block->Delete(g->nodeptr_block, np);
					if (!g->orphan_first) {
						g->orphan_last = NULL;
					}
					if (i->is_sink) {
						g->process_sink_orphan(g, i);
					} else {            
						g->process_source_orphan(g, i);
					}
				}

				g->orphan_first = np_next;
			}
			/* adoption end */
		}
		else 
			current_node = NULL;
	}
	// test_consistency();

	if (!reuse_trees || (g->maxflow_iteration % 64) == 0) {
		free(g->nodeptr_block); 
		g->nodeptr_block = NULL; 
	}
	g->maxflow_iteration ++;
	return g->flow;
}
void Graph_test_consistency(struct Graph *g, Node* current_node) {
	Node *i;
	struct Arc *a;
	int r;
	int num1 = 0, num2 = 0;

	// test whether all nodes i with i->next!=NULL are indeed in the queue
	for (i = g->nodes; i < g->node_last; i++) {
		if (i->next || i == current_node) {
			num1 ++;
		}
	}
	for (r = 0; r < 3; r++) {
		i = (r == 2) ? current_node : g->queue_first[r];
		if (i) {
			for ( ; ; i = i->next) {
				num2++;
				if (i->next == i) {
				}
			}
		}
	}

	for (i = g->nodes; i < g->node_last; i++) {
		// test whether all edges in seach trees are non-saturated
		if (i->parent == NULL) {}
		else if (i->parent == ORPHAN) {}
		else if (i->parent == TERMINAL) {
		} else {
		}
		// test whether passive nodes in search trees have neighbors in
		// a different tree through non-saturated edges
		if (i->parent && !i->next) {
			if (!i->is_sink) {
				for (a = i->first; a; a = a->next) {
				}
			} else	{
				for (a = i->first; a; a = a->next) {
				}
			}
		}
		// test marking invariants
		if (i->parent && i->parent != ORPHAN && i->parent != TERMINAL) {
		}
	}
}

/****************************************************************
 ******************            Graphcut        ******************
 ****************************************************************/
Graph *g_graph = 0;
void delete_graph() {
	if (g_graph != 0) {
		g_graph->GraphDeconstruct(g_graph); //Don't forget this.
		free(g_graph);
		g_graph = 0;
	}
}

void create_graph(int node_num, int edge_num) {
	delete_graph();

	g_graph = (Graph *)malloc(sizeof(Graph));
	Func_Graph(g_graph);
	g_graph->GraphConstruct(g_graph, node_num, edge_num, NULL);
}

void graph_add_node() {
	if (g_graph != 0) {
		g_graph->add_node(g_graph, 1); //default 1
	}
}

void graph_add_tweights(int node_index, float weight_source, float weight_sink) {
	if (g_graph != 0) {
		g_graph->add_tweights(g_graph, node_index, weight_source, weight_sink);
	}
}

void graph_add_edge(int node1_index, int node2_index, float weight_1to2, float weight_2to1) {
	if (g_graph != 0) {
		g_graph->add_edge(g_graph, node1_index, node2_index, weight_1to2, weight_2to1);
	}
}

float graph_max_flow() {
	return g_graph->maxflow(g_graph, 0, 0);//default false, 0
}

int graph_get_segment(int node_index) {
	if (g_graph->what_segment(g_graph, node_index, SOURCE) == SOURCE) {
		return 0;
	} else {
		return 1;
	}
}